//
// Created by lijia on 2021/5/15.
//

#include "setup.h"
#include "stm32f1xx_hal.h"
#include "display.h"
#include "joystick.h"
#include "servo.h"
#include "system.h"
#include "blinker.h"
#include "gps.h"
#include "solver.h"

#define SKIP_INITIAL_ZEROING 1

DisplayInfo *info = NULL;
GpsData *gpsData = NULL;
JoystickState *joystickState = NULL;

void system_setup() {
	solver_init();
	blinker_on();
	HAL_Delay(1000);
	info = display_get_info();
	joystickState = (JoystickState *) joystick_get();
	gpsData = (GpsData *) gps_get_last_frame();
	display_clear();
	display_printf("Calibrating joystick");
	HAL_Delay(100);
	joystick_calibrate_neutral();
	while (joystick_is_calibrating()) {
		joystick_tick();
	}
	display_clear();
	display_printf("Joystick calibrated");
	servo_init();
	display_clear();
	servo_arm();
	display_clear();
#if !(SKIP_INITIAL_ZEROING)
	display_printf("Servo zeroing");
	servo_zeroing();
	HAL_Delay(5000);
	display_clear();
	display_printf("Servo fulling");
	servo_fulling();
	HAL_Delay(5000);
	display_clear();
	servo_zeroing();
	solver_init();
#endif
	display_printf("Module state: %02x", system_get_module_status());
	HAL_Delay(1000);
	blinker_blink(10);
}
